package simpleMesh;

import hdgeom.primitives.Box;
import hdgeom.primitives.Line;
import hdgeom.primitives.Plane;

import java.awt.geom.Line2D;
import java.util.ArrayList;

import processing.core.PGraphics;
import processing.core.PMatrix3D;
import processing.core.PVector;
import simpleGeom.HasPos3D;

public class TrianglePVectorPrecalc extends Face{
	public Box bounds;
	PVector u;
	PVector v;
	PVector n;
	float d0;
	double SMALL_NUM = 0.00000001; // anything that avoids division overflow
	public TrianglePVectorPrecalc(PVector p0,PVector p1, PVector p2){
		this.addNode(p0);
		this.addNode(p1);
		this.addNode(p2);
		init();
	}
	
	@Override
	public PVector normal() {
		// TODO Auto-generated method stub
		return n;
	}

	public void init(){
	     u=PVector.sub(getNode(1), getNode(0));
	     v=PVector.sub(getNode(2), getNode(0));
	     n=u.cross(v);
	     n.normalize();
	     d0=-n.dot(getNode(0)); 
	     bounds=super.getBounds();
	}
	
	
	// intersect_RayTriangle(): intersect a ray with a 3D triangle
//	    Input:  a ray R, and a triangle T
//	    Output: *I = intersection point (when it exists)
//	    Return: -1 = triangle is degenerate (a segment or point)
//	             0 = disjoint (no intersect)
//	             1 = intersect in unique point I1
//	             2 = are in the same plane
	
	@Override
	public Box getBounds() {
		// TODO Auto-generated method stub
		return bounds;
	}

	public PVector intersectLineTriangle( PVector r0,PVector r1,boolean start, boolean end )
	{
	     // triangle vectors
		PVector i=linePlaneIntersection(r0,r1,start,end);
	    if (i==null)return null;
	    // is I inside T?
	    double    uu, uv, vv, wu, wv, D;
	    uu = PVector.dot(u,u);
	    uv = PVector.dot(u,v);
	    vv = PVector.dot(v,v);
	    PVector w =PVector.sub(i, getNode(0)) ;
	    wu = PVector.dot(w,u);
	    wv =PVector.dot(w,v);
	    D = uv * uv - uu * vv;
	    if (Math.abs(D)<SMALL_NUM)return null;//don't know why
	    
	    // get and test parametric coords
	    double s, t;
	    s = (uv * wv - vv * wu) / D;
	    if (s < 0.0 || s > 1.0)        // I is outside T
	        return null;
	    t = (uv * wu - uu * wv) / D;
	    if (t < 0.0 || (s + t) > 1.0)  // I is outside T
	        return null;

	    return i;                      // I is in T
	}
	
	public PVector intersect_RayTriangle( PVector r0,PVector r1 )
	{
		return intersectLineTriangle(r0,r1,true,false);
	}
	public PVector intersectSegTriangle( PVector r0,PVector r1 )
	{
		return intersectLineTriangle(r0,r1,true,true);
	}
	
	public PVector linePlaneIntersection(PVector r0,PVector r1,boolean start, boolean end){
		PVector dir=PVector.sub(r1,r0);
		 PVector  w0=PVector.sub(r0, getNode(0));
		 double   a = -1f*PVector.dot(n,w0);
		 double    b = PVector.dot(n,dir);
		    if (Math.abs(b) < SMALL_NUM) {     // ray is parallel to triangle plane
		        if (a == 0)                // ray lies in triangle plane
		            return null;
		        else return null;             // ray disjoint from plane
		    }
		    // get intersect point of ray with triangle plane
		    double  r = a / b;
		    if (start&&r < 0.0)return null;                    // ray goes away from triangle
		                         // => no intersect
		    if (end&&r>1.0)return null;
		   return PVector.add(r0, PVector.mult(dir,(float) r));
	}
	public PVector rayPlaneIntersection(PVector r0,PVector r1){
		return linePlaneIntersection(r0,r1,true,false);
	}
	public PVector segPlaneIntersection(PVector r0,PVector r1){
		return linePlaneIntersection(r0,r1,true,true);
	}
	public float distanceToPlane(float x, float y, float z){
			return n.dot(x, y, z)+d0;
	}
	
	public float getDistanceSqr(float x, float y, float z) {
		PMatrix3D m = new PMatrix3D();
		PVector planeAxis = new PVector(0, 0, 1);
		

		PVector v1 = getNode(0);
		PVector v2 = getNode(1);
		PVector v3 = getNode(2);
		PVector v12 = PVector.sub(v2, v1);
		PVector v13 = PVector.sub(v3, v1);
		PVector n = v12.cross( v13);
		n.normalize();
		float angle = (float) Math.acos(n.dot(planeAxis));

		PVector u = n.cross(planeAxis);
		u.normalize();

		float c = (float) Math.cos(angle);
		float s = (float) Math.sin(angle);
		float t = 1f - c;

		m.m00 = c + u.x * u.x * t;
		m.m01 = u.x * u.y * t - u.z * s;
		m.m02 = u.x * u.z * t + u.y * s;

		m.m10 = u.y * u.x * t + u.z * s;
		m.m11 = c + u.y * u.y * t;
		m.m12 = u.y * u.z * t - u.x * s;

		m.m20 = u.z * u.x * t - u.y * s;
		m.m21 = u.z * u.y * t + u.x * s;
		m.m22 = c + u.z * u.z * t;

		
		float[] f1 = new float[] { v1.x, v1.y, v1.z };
		float[] f2 = new float[] { v2.x, v2.y, v2.z };
		float[] f3 = new float[] { v3.x, v3.y, v3.z };

		float[] f1T = m.mult(f1, null);
		float[] f2T = m.mult(f2, null);
		float[] f3T = m.mult(f3, null);

		Line l1 = new Line(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1], f2T[2]);
		Line l2 = new Line(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1], f3T[2]);
		Line l3 = new Line(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1], f1T[2]);

		float minD = Float.MAX_VALUE;
		float[] pt = new float[] { x, y, z };
		float[] trans = new float[3];

		trans = m.mult(pt, null);
		float cX = trans[0];
		float cY = trans[1];
		float cZ = trans[2];
	
		int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
		int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
		int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
		float cDistance;
		if (r1 == r2 && r1 == r3) {
			cDistance = (cZ - l1.z1) * (cZ - l1.z1);
			if (cDistance < minD) {
				minD = cDistance;
			}
		} else {
			float d1 = (float) l1.distToSegSqr(cX, cY, cZ);
			float d2 = (float) l2.distToSegSqr(cX, cY, cZ);
			float d3 = (float) l3.distToSegSqr(cX, cY, cZ);
			if (d1 < minD) {
				minD = d1;
			}
			if (d2 < minD) {
				minD = d2;
			}
			if (d3 < minD) {
				minD = d3;
			}
		}

		return (float) minD;

	}
	public float getDistance(float x, float y, float z) {
		return (float) Math.sqrt(getDistanceSqr(x,y,z));
	}
	public  PVector intersect_VerRayTriangle( float x,float y,float z )
	{ 
		
		PVector r0=new PVector(x,y,z);
	   // sameNormal
		PVector p0=this.getNode(0);
		PVector p1=this.getNode(1);
		PVector p2=this.getNode(2);
	    if (Line2D.relativeCCW( p0.x, p0.y,  p1.x, p1.y,p2.x, p2.y)==1){
	    	PVector t=p0;
	    	p0=p1;
	    	p1=t;
	    }
	 
	   if (Line2D.relativeCCW( p2.x, p2.y,  p0.x, p0.y,r0.x, r0.y)==1)return null;	   
	   if (Line2D.relativeCCW( p0.x, p0.y,  p1.x, p1.y,r0.x, r0.y)==1)return null;
	   if (Line2D.relativeCCW( p1.x, p1.y,  p2.x, p2.y,r0.x, r0.y)==1)return null;  
	   Plane plane=new Plane(p2,p1,p0);
	    float d=plane.getDistance(r0);
	    if (d<0)return new PVector();
	    else return null;
	}
	public static PVector intersect_VerRayTriangle( PVector p0,PVector p1,PVector p2,PVector r0 )
	{ 
	   // sameNormal
	    if (Line2D.relativeCCW( p0.x, p0.y,  p1.x, p1.y,p2.x, p2.y)==1){
	    	PVector t=p0;
	    	p0=p1;
	    	p1=t;
	    }
	 
	   if (Line2D.relativeCCW( p2.x, p2.y,  p0.x, p0.y,r0.x, r0.y)==1)return null;	   
	   if (Line2D.relativeCCW( p0.x, p0.y,  p1.x, p1.y,r0.x, r0.y)==1)return null;
	   if (Line2D.relativeCCW( p1.x, p1.y,  p2.x, p2.y,r0.x, r0.y)==1)return null;  
	 
	 
	 Plane plane=new Plane(p2,p1,p0);
	    float d=plane.getDistance(r0);
	    if (d<0)return new PVector();
	    else return null;
	
	}
	public int isEdgesIntersectWithTriangle(TrianglePVectorPrecalc tri){
		int nIntersections=0;
		for (int i=0;i<3;i++){
			PVector v1=get(i);
			PVector v2=get((i+1)%3);
			if (tri.intersectSegTriangle(v1, v2)!=null)nIntersections++;
		}
		return nIntersections;
	}
	public static float getTriangleArea(PVector a, PVector b,PVector c){
		return getTriangleArea(a.x,a.y,a.z,b.x,b.y,b.z,c.x,c.y,c.z);
	}
	public static float getTriangleArea(float xa, float ya, float za, float xb, float yb, float zb, float xc, float yc, float zc){
		return (float)(0.5f*Math.sqrt(Math.pow(determinant(xa,xb,xc,ya,yb,yc,1,1,1),2)+Math.pow(determinant(ya,yb,yc,za,zb,zc,1,1,1),2)+Math.pow(determinant(za,zb,zc,xa,xb,xc,1,1,1),2)))
		;
	}
	public static float determinant(float a,float b,float c, float d,float e,float f, float g, float h, float i){
		 return (a*e*i-a*f*h-b*d*i+b*f*g+c*d*h-c*e*g);
		 }
	public float getAngle(PVector n1,PVector n2){
		return (float)Math.atan2(n1.cross(n2).mag(),PVector.dot(n1, n2));
	}
	public boolean isLine(float dAngle){
		PVector v1=PVector.sub(get(1), get(0));
		PVector v2=PVector.sub(get(2), get(0));
		float angle=getAngle(v1,v2);
		if (angle<dAngle||angle>Math.PI-dAngle){
			return true;//parallel planes
		}
		return false;
	}
	public boolean isIntersectWithPlane(Plane plane){
		float small=0.001f;
		float smallAngle=0.0001f;
		PVector n1=this.n;
		PVector n2=plane.normal;
		float angle=getAngle(n1,n2);
		//System.out.println("angle: "+angle);
		
		if (angle<smallAngle||angle>Math.PI-smallAngle){
			return false;//parallel planes
		}
		//if ()
		//angle = atan2(norm(cross(a,b)), dot(a,b))
		
		
		int edgeIntersect=0;
		float[]dist=new float[3];
		for (int i=0;i<3;i++){
			dist[i]=plane.getDistance(get(i));			
		}
		for (int i=0;i<3;i++){
			int i2=(i+1)%3;
			if (Math.abs(dist[i])>=small&&Math.abs(dist[i2])>=small){
				if (dist[i]>0!=dist[i2]>0)edgeIntersect++;
			}
		}
		if (edgeIntersect>0&&edgeIntersect<3)return true;
		return false;
	}
	public ArrayList<TrianglePVectorPrecalc> splitWithPlane(Plane plane) {
		float small=0.01f;
		
		ArrayList<TrianglePVectorPrecalc> newFaces = new ArrayList<TrianglePVectorPrecalc>();
		ArrayList<Integer>indexVertex=new ArrayList<Integer>();
		ArrayList<Integer>indexEdge=new ArrayList<Integer>();
		ArrayList<PVector>intersections=new ArrayList<PVector>();
		float[]dist=new float[3];
		for (int i=0;i<3;i++){
			dist[i]=plane.getDistance(get(i));			
		}
		for (int i=0;i<3;i++){
			int i2=(i+1)%3;
			if (Math.abs(dist[i])<small){
				indexVertex.add(i);
			}
			else  if (Math.abs(dist[i2])>=small){
				if (dist[i]>0!=dist[i2]>0){
					PVector v= plane.segmentPlaneIntersection(get(i), get(i2));
					indexEdge.add(i);
					intersections.add(v);
				}
			}
		}
		if (indexVertex.size()==1&&indexEdge.size()==1){
			int i=indexVertex.get(0);
			int i2=(i+1)%3;
			int i3=(i+2)%3;
			newFaces.add(new TrianglePVectorPrecalc(get(i),get(i2),intersections.get(0)));
			newFaces.add(new TrianglePVectorPrecalc(intersections.get(0),get(i3),get(i)));
		}
		else if (indexEdge.size()==2){
			int e1=indexEdge.get(0);
			int e2=indexEdge.get(1);
			PVector v1=intersections.get(0);
			PVector v2=intersections.get(1);
			if (e1==0&&e2==1){
				newFaces.add(new TrianglePVectorPrecalc(v1,get(1),v2));
				newFaces.add(new TrianglePVectorPrecalc(v2,get(2),get(0)));
				newFaces.add(new TrianglePVectorPrecalc(get(0),v1,v2));
			}
			else if (e1==1&&e2==2){
				newFaces.add(new TrianglePVectorPrecalc(v1,get(2),v2));
				newFaces.add(new TrianglePVectorPrecalc(v2,get(0),get(1)));
				newFaces.add(new TrianglePVectorPrecalc(get(1),v1,v2));
			}
			else if (e1==0&&e2==2){
				newFaces.add(new TrianglePVectorPrecalc(v2,get(0),v1));
				newFaces.add(new TrianglePVectorPrecalc(v1,get(1),get(2)));
				newFaces.add(new TrianglePVectorPrecalc(get(2),v2,v1));
			}
		}
		return newFaces;
	}
	public boolean edgeLengthZero(float small){
		for (int i = 0; i < vertices.size(); i++) {
			PVector v1 = vertices.get(i);
			PVector v2 = vertices.get((i+1)%3);
			if (v1.dist(v2)<small)return true;
		}
		return false;
	}
	public void display(PGraphics g) {
		g.beginShape(PGraphics.TRIANGLES);
		for (int i = 0; i < vertices.size(); i++) {
			PVector v = vertices.get(i);
			g.vertex(v.x, v.y, v.z);
		}
		g.endShape();
	}
}
